#include "app_main.h"                   //主应用头文件
#include "app/chassis/chassis.hpp"      //底盘控制
#include "app/robotcmd/robotcmd.hpp"    //机器人命令
#include "bsp/system/mutex.hpp"         //互斥锁
#include "cmsis_os.h"                   //CMSIS-RTOS API

#include "bsp/log/log.hpp"              //日志系统
#include "bsp/system/time.hpp"          //时间相关

#include "gimbal/gimbal.hpp"            //云台控制
#include "modules/imu/ins_task.h"       //IMU任务
#include "modules/motor/motor.hpp"      //电机模块
#include <memory>                       //智能指针
//!<未测试
#include "shoot/shoot.hpp"              //发射结构      添加时间2025/07/07

std::shared_ptr<float>    gimbal_yaw_motor_angle_ptr = nullptr;     //云台yaw轴共享指针（空指针）
std::shared_ptr<RobotCMD> robotcmd                   = nullptr;     //机器人命令对象
std::shared_ptr<Chassis>  chassis                    = nullptr;     //底盘对象
std::shared_ptr<Gimbal>   gimbal                     = nullptr;     //云台对象
attitude_t*               imu_data                   = nullptr;     //IMU数据指针

std::shared_ptr<Shoot>     shoot                     = nullptr;     //发射结构 /添加时间2025/07/07

void RobotCMDTask(void* arg __attribute__((unused)));               //机器人命令任务
void ChassisTask(void* arg __attribute__((unused)));                //底盘任务
void INSTask(void* arg __attribute__((unused)));                    //IMU任务
void GimbalTask(void* arg __attribute__((unused)));                 //云台任务

void ShootTask(void* arg __attribute__((unused)));                  //发射任务

void app_main()         
{
    taskENTER_CRITICAL();                //进入临界区(关闭中断，防止中断影响操作系统运行)
    RTTLog::init();                      //初始化实时日志系统
    STM32TimeDWT::DWT_Init(168);         //初始化高精度计时器(168MHz时钟)
    LOGINFO("Robot", "System Init");     //记录系统启动日志
    STM32CAN_Init();                     //初始化CAN总线
    StartDaemonTask();                   //启动守护任务
    StartMotorControlTask();             //启动电机控制任务
//创建全局共享对象
    gimbal_yaw_motor_angle_ptr = std::make_shared<float>(0.0f);     //云台(yaw)角度，初值为0
    chassis                    = std::make_shared<Chassis>();       //底盘实例
    imu_data                   = INS_Init();                        //初始化IMU并获取数据指针
    robotcmd                   = std::make_shared<RobotCMD>(imu_data, gimbal_yaw_motor_angle_ptr);  //命令模块
    gimbal                     = std::make_shared<Gimbal>(imu_data, gimbal_yaw_motor_angle_ptr);    //云台模块

    shoot                      = std::make_shared<Shoot>();       //发射任务 

//创建ROTS任务
    xTaskCreate(RobotCMDTask, "RobotCMDTask", 1024, nullptr, osPriorityNormal, nullptr);    //机器人命令任务，普通优先级，非最高非最低 
    xTaskCreate(ChassisTask, "ChassisTask", 1024, nullptr, osPriorityNormal, nullptr);      //底盘任务
    xTaskCreate(INSTask, "INSTask", 1024, nullptr, osPriorityNormal, nullptr);              //IMU任务
    xTaskCreate(GimbalTask, "GimbalTask", 1024, nullptr, osPriorityNormal, nullptr);        //云台任务

    xTaskCreate(ShootTask, "ShootTask", 1024, nullptr, osPriorityNormal, nullptr);         //发射任务

    taskEXIT_CRITICAL();                  //退出临界值(恢复中断)
    LOGINFO("Robot", "Robot Init");       //记录初始化完成日志
}

void RobotCMDTask(void* arg __attribute__((unused)))        //机器人控制任务
{
    while (true)                
    {
        robotcmd->RobotCMDTask();           //执行处理任务
        vTaskDelay(2 / portTICK_PERIOD_MS); //延时2s
    }
}

void ChassisTask(void* arg __attribute__((unused)))         //底盘任务
{
    while (true)
    {
        chassis->ChassisTask();
        vTaskDelay(5 / portTICK_PERIOD_MS);
    }
}

void INSTask(void* arg __attribute__((unused)))             //IMU任务
{
    while (true)
    {
        INS_Task();
        vTaskDelay(1 / portTICK_PERIOD_MS);
    }
}

void GimbalTask(void* arg __attribute__((unused)))          //云台任务
{
    while (true)
    {
        gimbal->GimbalTask();
        vTaskDelay(2 / portTICK_PERIOD_MS);
    }
}


void ShootTask(void* arg __attribute__((unused)))          //射击任务
{
    while (true)
    {
        shoot->ShootTask();
        vTaskDelay(2 / portTICK_PERIOD_MS);
    }
}